/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: IOClient.cpp,v 1.13 2009-10-23 13:56:43 morsko Exp $
 *
 * \file
 * \brief Implementation for IO interface client library.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 */

#include "IOClient.hpp"
#include "IOData.hpp"
#include <float.h>
//*****************************************************************************
using namespace MaCI::IO;
//*****************************************************************************
//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
CIOClient::CIOClient(gimi::GIMI *aGIMIPtr, const int aInterfaceMinor)
  : CSync(4,1),
    CThread(1),
    CMaCI(aGIMIPtr, GIMI_PROTOCOL_MACI_IO, aInterfaceMinor),
    iReplyMessages(),
    iCmdSequence(0)
{
}
//*****************************************************************************

CIOClient::~CIOClient()
{
  DoClose();
}
//*****************************************************************************

MaCI::EMaCIError CIOClient::DoOpen(void)
{
  MaCI::EMaCIError result = MaCI::KMaCIError;

  // Checks.
  if(!IsActive()) {

    // Do Subs. using the MaCI provided function.
    result = SubscribeToInterface();

    // If all inits were fine...
    if (result == KMaCIOK) {
 
      SetActive(true);
      CThread::RunThread();
      result = KMaCIOK;
    }
  }else{
    dPrint(3,"Client already opened");
  }

  if(result ==KMaCIOK){
    dPrint(3,"IOClient DoOpen was succesfull.");

  }

 return result;

}
//*****************************************************************************

MaCI::EMaCIError CIOClient::DoClose(void)
{
 MaCI::EMaCIError result = MaCI::KMaCIError;
 if(IsActive()){
   SetActive(false);
   CThread::SetRequestTermination();
   /* CThread::CancelThread(); //Removed by script */
   result = UnsubscribeToInterface(TimeoutValue(2000));

   Notify(KTypeDigitalPin);
   Notify(KTypeDigitalBlock);
   Notify(KTypeDigitalBlockMask);
   Notify(KTypeAnalogVoltage);

   CThread::WaitThread();
  
  
   if(result == KMaCIOK){
     dPrint(4,"Succesfully closed IOClient");    
   }else{
     dPrint(4,"Unsubscribing failed at IOClient!");
   }
 }else{
   dPrint(3,"IO client already closed!");
   result =  KMaCINotActive;
 }
 
 return result;

}
//*****************************************************************************

bool CIOClient::SetDigitalPin(const int aPin, const bool aState)
{
  gim::binbag::CBinBag bb;
  CIOData ioData;
  ioData.EncodeTo(&bb);
  ioData.SetCommand(TCommand(KCommandSetDigitalPin));
  ioData.SetDigitalPinState(TDigitalPinState(aPin,aState));

  //  ioData.Print(1);
  // Send the command
  const MaCI::EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
						  bb.GetBufferSize());
  
  return (e == MaCI::KMaCIOK);
  
}
//*****************************************************************************

bool CIOClient::SetDigitalMask(const int aBlock, 
			       const int aBlockState,
			       const int aBlockStateApplyMask)
{

  gim::binbag::CBinBag bb;
  CIOData ioData;
  ioData.EncodeTo(&bb);
  ioData.SetCommand(TCommand(KCommandSetDigitalBlock));
  TDigitalBlockState bs(aBlock,aBlockState);
  TDigitalBlockMask bm(bs,
		       aBlockStateApplyMask);
  ioData.SetDigitalBlockMask(bm);

  // Send the command
  const MaCI::EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
                                            bb.GetBufferSize());
  
  return (e == MaCI::KMaCIOK);		     

}
//*****************************************************************************
		    
int CIOClient::GetDigitalPin(const int aPin,
			     unsigned int aTimeout_ms)
{

  const unsigned int begin = ownTime_get_ms();

  int timeLeft = 0;
  int result;
  int r;
  int messageId = 0;
  CIOData ioData;
  gim::binbag::CBinBag bb;
  r = ioData.EncodeTo(&bb);

  ioData.SetCommand(TCommand(KCommandGetDigitalPin));
  TDigitalPinState ps(aPin,0);
  ioData.SetDigitalPinState(ps);
  
  
  
  

  Lock();
  messageId = ++iCmdSequence;
  Unlock();

  // Send the message to default target with default major & minor numbers.
  dPrint(10,"Sending request get digital pin");
  r = iGIMIPtr->send(iDefaultTarget,
                     bb.GetBuffer(),
                     bb.GetBufferSize(),
                     iInterfaceMajor,
                     iInterfaceMinor,
		     messageId);
  dPrint(10,"Waiting messageId %d", messageId);
  Lock();
  switch(r) {
  case GIMI_OK:{
    
    // Message sent OK. Waiting for reply
    timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    std::map<int,CIOData *>::iterator iter = iReplyMessages.end();
    while (timeLeft > 0 && ( iter = iReplyMessages.find(messageId) ) == iReplyMessages.end()) {
      Wait(timeLeft,KTypeDigitalPin);
      
      //Check for timeout
      timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    }
    if(iter != iReplyMessages.end()){
      //get the reply data and delete it from map
      
      result = iter->second->GetDigitalPinState()->state;
      
      delete iter->second;
      
      iReplyMessages.erase(iter);      
    }else{
      dPrint(3,"No result within timeout");
      result = -1;
    }
    break;
  }
  case GIMI_NOCONNECTION:{
    dPrint(3,"NO CONNECTION ");
    result = -1;
    
    break;
  }
  case GIMI_INTERRUPT:
  case GIMI_ERROR:
  default:
    // Any error, print warning and sleep for one second.
    dPrint(6,"GIMI Error (%d, '%s') - from send() received",
           r, gimi::getGimiErrorString(r).c_str());
    result =-1;
    break;
  }
  Unlock();
  


  return result;


}
//*****************************************************************************

int CIOClient::GetDigitalMask(const int aBlock,
			      unsigned int aTimeout_ms)
{
  const unsigned int begin = ownTime_get_ms();
  int result;
  int messageId = 0;
  int timeLeft = 0;
  int r;
  gim::binbag::CBinBag bb;
  CIOData ioData;
  ioData.EncodeTo(&bb);
  ioData.SetCommand(TCommand(KCommandGetDigitalBlock));
  TDigitalBlockState bs(aBlock,0);
  ioData.SetDigitalBlockState(bs);

  Lock();
  messageId = ++iCmdSequence;
  Unlock();

  // Send the message to default target with default major & minor numbers.
  r = iGIMIPtr->send(iDefaultTarget,
                     bb.GetBuffer(),
                     bb.GetBufferSize(),
                     iInterfaceMajor,
                     iInterfaceMinor,
		     messageId);
  Lock(); 
  switch(r) {
  case GIMI_OK:{
    // Message sent OK. Waiting for reply
    timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    std::map<int,CIOData *>::iterator iter = iReplyMessages.end();
 
    while (timeLeft > 0 && ( iter = iReplyMessages.find(messageId) ) == iReplyMessages.end()) {
      Wait(timeLeft,KTypeDigitalBlock);
      //Check for timeout
      timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    }
    if(iter != iReplyMessages.end()){
   
     //get the value and delete data from map
     
      result = iter->second->GetDigitalBlockState()->state;
      delete iter->second;
      iReplyMessages.erase(iter);      

    }else{
      dPrint(3,"No result within timeout");
      result = -1;
    }
    break;
    break;
  }
  case GIMI_NOCONNECTION:{
    dPrint(3,"NO CONNECTION ");
    result = -1;
    
    break;
  }
  case GIMI_INTERRUPT:
  case GIMI_ERROR:
  default:
    // Any error, print warning and sleep for one second.
    dPrint(6,"GIMI Error (%d, '%s') - from send() received",
           r, gimi::getGimiErrorString(r).c_str());
    result =-1;
    break;
  }
  Unlock();
  return result;


}
//*****************************************************************************

float CIOClient::GetAnalogVoltage(const int aChannel,
				  unsigned int aTimeout_ms)
{
  const unsigned int begin = ownTime_get_ms();
  int timeLeft = 0;
  float result;
  int r;
  gim::binbag::CBinBag bb;
  int messageId = 0;
  CIOData ioData;
  ioData.EncodeTo(&bb);
  ioData.SetCommand(TCommand(KCommandGetAnalogVoltage));
  TAnalogVoltage av(aChannel,0);
  ioData.SetAnalogVoltage(av);

  Lock();
  messageId = ++iCmdSequence;
  Unlock();

  // Send the message to default target with default major & minor numbers.
  r = iGIMIPtr->send(iDefaultTarget,
                     bb.GetBuffer(),
                     bb.GetBufferSize(),
                     iInterfaceMajor,
                     iInterfaceMinor,
		     messageId);
  Lock();
  switch(r) {
  case GIMI_OK:{
    // Message sent OK. Waiting for reply
    timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    std::map<int,CIOData *>::iterator iter =iReplyMessages.end();
    while (timeLeft > 0 && ( iter = iReplyMessages.find(messageId) ) == iReplyMessages.end()) {
      Wait(timeLeft,KTypeAnalogVoltage);
      //Check for timeout
      timeLeft = aTimeout_ms - ownTime_get_ms_since(begin);
    }
    if(iter != iReplyMessages.end()){
      //get the value and delete it from map
    
      result = iter->second->GetAnalogVoltage()->voltage;
      delete iter->second;
      iReplyMessages.erase(iter);      
    }else{
      dPrint(3,"No result within timeout");
      result = -1;
    }
    break;
   
  }
  case GIMI_NOCONNECTION:{
    dPrint(3,"NO CONNECTION ");
    result = FLT_MAX;
    break;
  }
  case GIMI_INTERRUPT:
  case GIMI_ERROR:
  default:
    // Any error, print warning and sleep for one second.
    dPrint(6,"GIMI Error (%d, '%s') - from send() received",
           r, gimi::getGimiErrorString(r).c_str());
    result = FLT_MAX;
    break;
  }
  Unlock();
    
  return result;
  

}
//*****************************************************************************

bool CIOClient::SetAnalogVoltage(const int aChannel, const double aVoltage)
{
  dPrint(10,"SetAnalogVoltage called with %d %f", aChannel,aVoltage);
  gim::binbag::CBinBag bb;
  CIOData ioData;
  ioData.EncodeTo(&bb);
  ioData.SetCommand(TCommand(KCommandSetAnalogVoltage));
  TAnalogVoltage av(aChannel,aVoltage);
  ioData.SetAnalogVoltage(av);


  // Send the command
  const EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
                                            bb.GetBufferSize());
  
  return (e == KMaCIOK);

}
//*****************************************************************************

/// Thread runner function. Is in charge of receiving events.
int CIOClient::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  EMaCIError e;
  gimi::GIMIMessage gmsg;
  
  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(4, "GIMIMessage handler started");
  
  // Run as long as the thread should be active
  while(IsActive()) {
    // Call the ReceiveMessage wrapper with 1s timeout
    e = ReceiveInterfaceMessage(gmsg, 1000*60*60*25);
    if (e == KMaCIOK) {

      ProcessMessage(gmsg);


    } else if (e != KMaCITimeout) {
      // Other error, try to recover by sleeping one second.
      if(IsActive()){
        dPrint(3,"ProcessMessage returned error %u and client still active. Trying to recover by sleeping 1000ms ",
             e);
        ownSleep_ms(1000);
      }else{
        dPrint(3,"ProcessMessage returned error %u.",
               e);
      }

     

      
    }
  }
  
  dPrint(4, "GIMIMessage handler stopped");

  return result;




}
//*****************************************************************************
int CIOClient::ProcessMessage(const gimi::GIMIMessage &msg)
{

  int result = 0;
  gim::binbag::CBinBag bb;
  CIOData d;
  bool r;


  r = bb.Decode(msg.getData(), msg.getDatalength());

  if (r) {
    // Succesfully decoded as BinBag.
    r = d.DecodeFrom(&bb);
    if (r) {
      // Succesfully decoded as SpeedCtrl
      // Now, Check whether it was Command.
      const TCommand *cmd = d.GetCommand();
      if (cmd) {
        switch(cmd->cmd) {
	case KCommandGetAnalogVoltage:{

	  CIOData *new_entry = new CIOData(d);

	  Lock();
	  iReplyMessages.insert(std::make_pair(msg.messageId,new_entry));
	  
	  Notify(KTypeAnalogVoltage);
	  Unlock();

	  break;
	}
	case KCommandGetDigitalBlock:{

	  CIOData *new_entry = new CIOData(d);

	  Lock();
	  iReplyMessages.insert(std::make_pair(msg.messageId,new_entry));
	 
	  Notify(KTypeDigitalBlock);
	  Unlock();

	  break;
	}
	case KCommandGetDigitalPin:{

	  CIOData *new_entry = new CIOData(d);

	  Lock();
	  iReplyMessages.insert(std::make_pair(msg.messageId,new_entry));
	
	  Notify(KTypeDigitalPin);
	  Unlock();  

	  break;
	}
	default:
          dPrint(4,"Unidentified command! %d (%x)! Ignore.",
                 cmd->cmd, cmd->cmd);
          break;
        }
      }
    }else{
      dPrint(5,"DecodeFrom failed");
    }
  }else{
    dPrint(5,"Decode failed");
  }


  return result;


}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
     
